﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace ABELGX
{
    public class CmdArm:CarCmd
    {
        public RegisterBool Arm1Up { get; set; } = new RegisterBool();
        public RegisterBool Arm1Down { get; set; } = new RegisterBool();
        public RegisterBool Arm2Up { get; set; } = new RegisterBool();
        public RegisterBool Arm2Down { get; set; } = new RegisterBool();
        public RegisterBool Arm3Up { get; set; } = new RegisterBool();
        public RegisterBool Arm3Down { get; set; } = new RegisterBool();
        public RegisterBool EndLevelUp { get; set; } = new RegisterBool();
        public RegisterBool EndLevelDown { get; set; } = new RegisterBool();
        public RegisterBool ArmExtend { get; set; } = new RegisterBool();
        public RegisterBool ArmRetract { get; set; } = new RegisterBool();
        public RegisterBool UnderLeft { get; set; } = new RegisterBool();
        public RegisterBool UnderRight { get; set; } = new RegisterBool();
        public RegisterBool NozzleUp { get; set; } = new RegisterBool();
        public RegisterBool NozzleDown { get; set; } = new RegisterBool();
        public RegisterBool NozzleLeft { get; set; } = new RegisterBool();
        public RegisterBool NozzleRight { get; set; } = new RegisterBool();
        public RegisterBool LegExtend { get; set; } = new RegisterBool();
        public RegisterBool LegRetract { get; set; } = new RegisterBool();
        public RegisterBool Arm1Auto { get; set; } = new RegisterBool();
        public RegisterBool Arm2Auto { get; set; } = new RegisterBool();
        public RegisterBool Arm3Auto { get; set; } = new RegisterBool();
        public RegisterBool LevelAuto { get; set; } = new RegisterBool();
        public RegisterBool FlexibleAuto { get; set; } = new RegisterBool();
        public RegisterBool UnderAuto { get; set; } = new RegisterBool();
        public RegisterBool EndAuto { get; set; } = new RegisterBool();
        public RegisterBool Work { get; set; } = new RegisterBool();
        public double Arm1Angle { get; set; }
        public double Arm2Angle { get; set; }
        public double Arm3Angle { get; set; }
        public double LevelAngle { get; set; }
        public double Flexible { get; set; }
        public double UnderAngle { get; set; }
        public double EndPitchAngle { get; set; }
        public double EndTurnAngle { get; set; }
        public int Speed { get; set; } = 60;
        public int SpeedAuto { get; set; } = 60;
        public CmdArm(CmdSet set, int length) : base(set, length)
        {
            
        }
        public override byte[] GetCmd()
        {
            int i;
            for (i = 4; i < Cmd.Length; i++)
            {
                Cmd[i] = 0x00;
            }
            i = 4;

            Cmd[i] = (byte)(Arm1Up.Value ? 0x01 : 0x00);
            Cmd[i] |= (byte)(Arm1Down.Value ? 0x02 : 0x00);
            Cmd[i] |= (byte)(Arm2Up.Value ? 0x04 : 0x00);
            Cmd[i] |= (byte)(Arm2Down.Value ? 0x08 : 0x00);
            Cmd[i] |= (byte)(Arm3Up.Value ? 0x10 : 0x00);
            Cmd[i] |= (byte)(Arm3Down.Value ? 0x20 : 0x00);
            Cmd[i] |= (byte)(EndLevelUp.Value ? 0x40 : 0x00);
            Cmd[i] |= (byte)(EndLevelDown.Value ? 0x80 : 0x00);
            i++;

            Cmd[i] = (byte)(ArmExtend.Value ? 0x01 : 0x00);
            Cmd[i] |= (byte)(ArmRetract.Value ? 0x02 : 0x00);
            Cmd[i] |= (byte)(UnderLeft.Value ? 0x04 : 0x00);
            Cmd[i] |= (byte)(UnderRight.Value ? 0x08 : 0x00);
            Cmd[i] |= (byte)(NozzleUp.Value ? 0x10 : 0x00);
            Cmd[i] |= (byte)(NozzleDown.Value ? 0x20 : 0x00);
            Cmd[i] |= (byte)(NozzleLeft.Value ? 0x40 : 0x00);
            Cmd[i] |= (byte)(NozzleRight.Value ? 0x80 : 0x00);
            i++;

            Cmd[i] = (byte)(LegExtend.Value ? 0x01 : 0x00);
            Cmd[i] |= (byte)(LegRetract.Value ? 0x02 : 0x00);
            i++;

            Cmd[i] = (byte)(Arm1Auto.Value ? 0x01 : 0x00);
            Cmd[i] |= (byte)(Arm2Auto.Value ? 0x02 : 0x00);
            Cmd[i] |= (byte)(Arm3Auto.Value ? 0x04 : 0x00);
            Cmd[i] |= (byte)(LevelAuto.Value ? 0x08 : 0x00);
            Cmd[i] |= (byte)(FlexibleAuto.Value ? 0x10 : 0x00);
            Cmd[i] |= (byte)(UnderAuto.Value ? 0x20 : 0x00);
            Cmd[i] |= (byte)(EndAuto.Value ? 0x40 : 0x00);
            Cmd[i] |= (byte)(Work.Value ? 0x80 : 0x00);
            i++;

            short value = Convert.ToInt16(Arm1Angle * 100);
            Cmd[i++] = (byte)value;
            Cmd[i++] = (byte)(value >> 8);

            value = Convert.ToInt16(Arm2Angle * 100);
            Cmd[i++] = (byte)value;
            Cmd[i++] = (byte)(value >> 8);

            value = Convert.ToInt16(Arm3Angle * 100);
            Cmd[i++] = (byte)value;
            Cmd[i++] = (byte)(value >> 8);

            value = Convert.ToInt16(LevelAngle * 100);
            Cmd[i++] = (byte)value;
            Cmd[i++] = (byte)(value >> 8);

            value = Convert.ToInt16(Flexible);
            Cmd[i++] = (byte)value;
            Cmd[i++] = (byte)(value >> 8);

            value = Convert.ToInt16(UnderAngle * 100);
            Cmd[i++] = (byte)value;
            Cmd[i++] = (byte)(value >> 8);

            value = Convert.ToInt16(EndPitchAngle * 100);
            Cmd[i++] = (byte)value;
            Cmd[i++] = (byte)(value >> 8);

            value = Convert.ToInt16(EndTurnAngle * 100);
            Cmd[i++] = (byte)value;
            Cmd[i++] = (byte)(value >> 8);

            Cmd[i++] = (byte)Speed;
            Cmd[i++] = (byte)SpeedAuto;

            CalXOR(Cmd);
            return Cmd;
        }
    }
}
